#ifndef __MC_TRAJECTORY_H__
#define __MC_TRAJECTORY_H__

#include <Eigen/Geometry>
#include <deque>

using namespace std;

namespace mc{

// 定义Pose2D类
class Pose2D {
public:
    Eigen::Translation2d translation;   // x,y
    Eigen::Rotation2Dd rotation;        // θ

    // 构造函数
    explicit Pose2D(double x = 0, double y = 0, double theta = 0) 
        : translation(x, y), rotation(theta) {}

    // 转换为3x3齐次矩阵
    Eigen::Matrix3d toMatrix() const {
        Eigen::Matrix3d T = Eigen::Matrix3d::Identity();
        T.block<2,2>(0,0) = rotation.toRotationMatrix();
        T.block<2,1>(0,2) = translation.vector();
        return T;
    }
};

// 定义KinematicState2D
struct KinematicState2D {
    Eigen::Vector2d linear_velocity{Eigen::Vector2d::Zero()};
    double angular_velocity{0};
    
    Eigen::Vector2d linear_acceleration{Eigen::Vector2d::Zero()};
    double angular_acceleration{0};
    
    Eigen::Vector2d linear_jerk{Eigen::Vector2d::Zero()};
    double angular_jerk{0};
    
    // 提供重置接口
    void reset() { *this = KinematicState2D(); }
};

// 定义TrajectoryPoint2D
struct TrajectoryPoint2D{
    string frame_id;                //坐标系
    double time_from_start{0};      // 时间戳（相对于轨迹起点）    
    Pose2D pose{0, 0, 0};           // 位姿（包含x,y,θ）
    KinematicState2D kinematics{};  // 动力学属性（包含线速度、角速度、线加速度、角加速度、线加加速度）

    // 几何属性
    double curvature{0};              // 曲率 κ (1/m)
    double curvature_rate{0};         // 曲率变化率 dκ/dt (1/m²/s)

    // 构造方法
    TrajectoryPoint2D() = default;  // 使用默认构造

    explicit TrajectoryPoint2D(const Pose2D& p, double t = 0) 
        : pose(p), time_from_start(t) {}

    static TrajectoryPoint2D create(const Pose2D& p, double t) {
        return TrajectoryPoint2D(p, t);  // 返回值优化
    }
};

// 定义Trajectory
using Trajectory2D = std::deque<TrajectoryPoint2D>;  

};  // namespace mc

#endif